#include "CTorqueControl.h"

CML_NAMESPACE_USE();

CTorqueControl::CTorqueControl(void)
{
	f_control.open("control_record.txt",ios::trunc);
	f_measure.open("measure_record.txt",ios::trunc);
	outputLimit = 20;
	lastOutput = 0;
	diffOutput = 0;
	velLoad=0;
}

CTorqueControl::~CTorqueControl(void)
{
	
}

const Error *CTorqueControl::EnableAction( void )
{
	const Error *err=sa->SetAmpMode(AMPMODE_CAN_VELOCITY);
	CAction::EnableAction();
	return err;
}

void CTorqueControl::DoAct(void)
{
	delta[0] = sa->GetForceSet() - sa->GetForceMeasure();
	delta[1] = sa->GetVelForceSet() - sa->GetVelForceMeasure();
	if(sa->GetTimeInterval() > 0)
	{
		diffOutput = (Kd * delta[1] + Td / sa->GetTimeInterval() * lastOutput) / (1 + Td / sa->GetTimeInterval());
		lastOutput = diffOutput;
	}
	//velLoad=0.1*sa->GetVelLoad()+0.9*velLoad;
	output = (sa->GetVelForceSet()) / (sa->GetElasticCoefficient()) + Kp * delta[0] + diffOutput - sa->GetVelLoad()*0.97;//;
	if(output > outputLimit)
		output = outputLimit;
	else if(output<-outputLimit)
		output=-outputLimit;
	//output=output*0.97;

	sa->SetTargetVel(output * ENCODERNUM*RATIO / 2 / PI);
	//sa->SetTargetVel( 0);
	
	sa->GetVelocityActual(velMotorActual);

	

	f_control<<(sa->GetForceMeasure())<<"  ";
	f_control<<(sa->GetForceSet())<<"  ";
	f_control<<(Kd * delta[1])<<"  ";
	f_control<<(Kp * delta[0])<<"  ";
	f_control<<output<<"  ";
	f_control<<(velMotorActual/ENCODERNUM/RATIO*PI*2)<<"  ";
	f_control<<diffOutput<<endl;
	
	f_measure<<sa->GetPosSpring()<<"  ";
	f_measure<<sa->GetVelSpring()<<"  ";
	f_measure<<sa->GetAccSpring()<<"  ";
	
	f_measure<<sa->GetPosLoad()<<"  ";
	f_measure<<sa->GetVelLoad()<<"  ";
	f_measure<<sa->GetAccLoad()<<"  ";

	f_measure<<sa->GetPosMotor()<<"  ";
	f_measure<<sa->GetVelMotor()<<"  ";
	f_measure<<sa->GetAccMotor()<<"  ";
	f_measure<<Kp * delta[0]<<"  ";
	f_measure<<output<<"  ";
	f_measure<<sa->GetVelLoad()<<"  ";
	f_measure<<sa->GetForceMeasure()<<"  ";
	f_measure<<endl;
	return;
}
